/*
frigOS
Licensed under GPLv3
*/

#ifndef DISABLE_ADC

#include "adc.h"
#include "kernel.h"
#include "led.h"
#include "serial.h"
#include "timer.h"
#include "protocol.h"

#include <avr/io.h>
#include <avr/interrupt.h>

//volatile uint8_t battery[3];
//volatile uint8_t accelerometer[4];
volatile uint8_t channel = 0;
volatile int16_t adc_value = 0;

volatile int16_t fallenFront;
volatile int16_t fallenBack;
volatile int16_t standing;
volatile uint8_t fallenBackFlag;
volatile uint8_t fallenFrontFlag;

volatile uint16_t accel_lower_limit;
volatile uint16_t accel_upper_limit;
volatile uint8_t accel_lateral_channel;
volatile uint8_t accel_range_set;

void initADC(void)
{

    /*// input
    //DDRF &= ~(1 << DDF3);
    // enable pull up
    //PORTF |= (1 << PF3);

    //DDRF &= ~(1 << DDF2);
    //PORTF |= (1 << PF2);
    DDRF &= ~(1 << DDF1);
    PORTF |= (1 << PF1);
    //DDRF &= ~(1 << DDF0);
    //PORTF |= (1 << PF0);

    // ADC enable
    // ADC start conversion
    // ADC free running mode
    // set prescalar 128
    ADCSRA = (1 << ADEN) | (1 << ADSC) | (1 << ADFR) | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);

    // ADC3
    //ADMUX = (1 << MUX1) | (1 << MUX0);
    //ADMUX = (1 << MUX1);
    ADMUX = (1 << MUX0);
    //ADMUX = 0;*/

    accel_range_set = 1;
    accel_lower_limit = 275;
    accel_upper_limit = 500;
    accel_lateral_channel = 1;
    fallenBackFlag = 0;
    fallenFrontFlag = 0;
    fallenFront = 0;
    fallenBack = 0;

    // input
    DDRF &= ( ~ ( (1 << DDF3) | (1 << DDF1) | (1 << DDF0) ) );
    // enable pull up
    PORTF |= ( (1 << PF3) | (1 << PF1) | (1 << PF0) );

    // ADC enable
    // ADC start conversion
    // ADC free running mode
    // set prescalar 128
    ADCSRA = (1 << ADEN) | (1 << ADSC) | (1 << ADFR) | (1 << ADIE) | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);

    // ADC0
    ADMUX = (1 << REFS1) | (1<< REFS0);

} // initADC

void adcPause()
{
    ADCSRA = ADCSRA & (~(1 << ADIE));
}

void adcResume()
{
    ADCSRA = ADCSRA | (1 << ADIE);
}

/*void testAccelerometer(void *arg) {


	initADC();

	int16_t adc_value;
	uint8_t channel = 0;
        char buf[25];
	while(1) {

		while( !ADCSRA & (1 << ADIF) );
			asm volatile("nop"::);

		adc_value = ADC;
		accelerometer[0] = channel;
		accelerometer[1] = (uint8_t) (adc_value >> 8);
		accelerometer[2] = (uint8_t) adc_value;
                accelerometer[3] = 0;
                if(channel == 3)
                {
                sprintf(&buf,"%d\n",adc_value);
                }
                else
                {
                  sprintf(&buf,"%d,",adc_value);
                }
		lockSemaphore(&usart1_lock);
		//transmitBufferUSART1( (uint8_t *) accelerometer, 4 );

                transmitStringUSART1(&buf);
		unlockSemaphore(&usart1_lock);

		switch(channel) {

			case 0:
				channel = 1;
				break;
			case 1:
				channel = 3;
				break;
			case 3:
				channel = 0;
				break;
			default:
				channel = 0;
				break;

		}

		ADMUX = channel | (1 << REFS1) | (1<< REFS0);

		sleep(100);

	}

} // accelerometer

void initUpButton(void) {

	DDRE &= ~(1 << DDE4);
	PORTE |= (1 << PE4);

	EIFR |= (1 << INTF4);
	EIMSK |= (1 << INT4);

} // initUpButton

void chargeBattery(void *arg) {

	int16_t adc_value;
	uint16_t start_time = ticks3;

	DDRB |= (1 << DDB5);
	PORTB |= (1 << PB5);

	initADC();

	while(1) {

		PORTB ^= (1 << PB5);

		adc_value = ADC;
		battery[0] = 1;
		battery[1] = (uint8_t) (adc_value >> 8);
		battery[2] = (uint8_t) adc_value;

		if ( (ticks3 - start_time) > 5 ) {

			manageLEDOn();

			lockSemaphore(&usart1_lock);
			transmitBufferUSART1( (uint8_t *) battery, 3 );
			unlockSemaphore(&usart1_lock);

			start_time = ticks3;

		}

	}

} // chargeBattery

void readAccelerometer(void *arg) {

	static uint8_t channel = 0;

	initAccelerometer();

	while (1) {

		while( !ADCSRA & (1 << ADIF) )
			asm volatile("nop"::);

		ADMUX =  (1 << channel);
		accelerometer[channel] = ADC;

		if (++channel > 2)
			channel = 0;

	}

} // readAccelerometer

void transmitTest(void *arg) {

	char letter = 'a';

	while(1) {

		for (uint8_t x = 0; x < 26; x++) {

			lockSemaphore(&usart1_lock);
			transmitBufferUSART1( (uint8_t *) &letter, 1);
			unlockSemaphore(&usart1_lock);
			letter++;
		}

		letter = 'a';

	}

} // transmitTest
*/
ISR(ADC_vect)
{
    adc_value = ADC;

    switch(channel)
    {
    case 0:
        channel = 1;
        break;

    case 1:
        channel = 3;
        break;

    case 3:
    default:
        channel = 0;
        break;
    }

    if((channel == accel_lateral_channel) && accel_range_set)
    {
        if(adc_value > accel_upper_limit)
        {
            fallenFront++;
            fallenBack--;
            standing--;
        }
        else if(adc_value < accel_lower_limit)
        {
            fallenBack++;
            fallenFront--;
            standing--;
        }
        else
        {
            standing++;
            fallenFront--;
            fallenBack--;
        }

        if (fallenFront < 0)
        {
            fallenFront = 0;
        }
        if (fallenBack < 0)
        {
            fallenBack = 0;
        }
        if (standing < 0)
        {
            standing = 0;
        }

        if(fallenFront > FALL_THRESHOLD)
        {
            fallenFrontFlag = 1;
            fallenBackFlag = 0;
            fallenFront = 0;
            //programLEDOn();
            //auxLEDOff();
        }
        if(fallenBack > FALL_THRESHOLD)
        {
            fallenBackFlag = 1;
            fallenFrontFlag = 0;
            fallenBack = 0;
            //auxLEDOn();
            //programLEDOff();
        }
        if (standing > FALL_THRESHOLD)
        {
            fallenBackFlag = 0;
            fallenFrontFlag = 0;
            standing = 0;
            //programLEDOff();
            //auxLEDOff();
        }
    }

    ADMUX = channel | (1 << REFS1) | (1<< REFS0);

    // CIB wake up tasks that were waiting for this event
    fireEvent(A2D_COMPLETE_EVENT);
}


/*void checkFallen(void * arg)
{
	uint8_t reply[6];
	cli();
	reply[0] = CMD_HEADER;
	reply[1] = fallenFrontFlag;
	reply[2] = fallenFrontFlag;
	reply[3] = fallenBackFlag;
	reply[4] = fallenBackFlag;
	reply[5] = protocolCheckSum(&reply[1], 4);
	sei();
	lockSemaphore(&usart1_lock);
	transmitBufferUSART1(reply,6);
	unlockSemaphore(&usart1_lock);
	fallenBackFlag = 0;
	fallenFrontFlag = 0;
}
*/
/*ISR(INT4_vect) {

	static uint8_t charging = 0;

	if (!charging) {

		//startThread( (void *) &transmitTest, 0, MAXIMUM_PRIORITY, MINIMUM_STACK_SIZE );
		startThread( (void *) &chargeBattery, 0, MEDIUM_PRIORITY, MINIMUM_STACK_SIZE );
		charging = 1;

	}

}*/

#endif
